#pragma once
#include "../_includes.h"

void costume_go(double,double,double,double,int);
void line_loop(double,float,float,int);
void line_output();

double get_gyro();
double get_gyro_rate();
double get_pitch();
double get_pitch_rate();
void gyro_reset();

// turn
bool is_turn_pid_adjust_done();
void turn_pid_adjust_agl_absolute(double aim_agl, double max_pwr, int timeout,
                                  Block waitForCompletion,
                                  double max_wheel_difference_adjust_pwr = 10);
//swing turn
bool is_swing_turn_pid_adjust_done();
void swing_turn_pid_adjust_agl_absolute(int side, double aim_agl,
                                        double max_pwr, int timeout,
                                        Block waitForCompletion);
void turn_pid_adjust_agl_relative(double delta_angle, double max_pwr,
                                  int timeout, Block waitForCompletion,
                                  double max_wheel_difference_adjust_pwr);                                        

// fast forward
void fast_forward_stop();
bool is_fast_forward_done();
void fast_forward_dist_relative(double aim_dist_mm, double max_pwr = 100,
                                int timeout = 0, bool waitForCompletion = true,
                                double max_adjust_pwr = 10);

// vision
#define VIS_MAX_X 316
#define VIS_MAX_Y 212

class Vis_Data {
public:
  int dx; // 视觉中心x差值
  int dy; // 视觉中心y差值
  Vis_Data(int dx0, int dy0) : dx(dx0), dy(dy0) {}
  friend ostream &operator<<(ostream &out, Vis_Data &d) {
    out << "(" << d.dx << ", " << d.dy << ")\n";
    return out;
  }
};

Vis_Data vis_get_lowest_cdx(vision &v, vex::vision::signature &sig);
void vis_forward_dist_relative(vision &v, signature &sig, int dist, int timeout,
                               double max_turn, double max_fwd);


// 关于倒退的函数
double speed_start(double aimdist, double aimspeed, double acc, double timeout);
void stable_go(double aimdist, double aimspeed, double acc, double timeout);
void fade_end(double aimdist, double aimspeed, double timeout);